/*  From Rapaport [2004], p.165-168 - working with the definition of the Langrange multiplier */
void ConstrainTemperature()
{
  VecR vt, uB, uS, wvB, trqB;
  real s1, s2, vFac, halfDeltaT = deltaT / 2.;
  int n;

  s1 = s2 = 0.;
  DO_SITE {
    VSAdd( vt, siteVelocity[ n ], halfDeltaT / mass[ site[ n ].type ], siteForce[ n ] ); // half-advance linear velocities
    s1 += VDot( vt, siteForce[ n ] );
    s2 += mass[ site[ n ].type ] * VLenSq( vt );
    if ( site[ n ].mechType == NSRB ) {
      MVMul( trqB, siteRotationMatrix[ n ].u, siteTorque[ n ] );   /* getting the body torque T^b = R * T^S */
      VSAdd( uB, siteAngularMomentum[ n ], halfDeltaT, trqB );            /* half-advance momenta of inertia (body principal frame) from t to t+dt/2 */
      VDiv( wvB, uB, ssdInertia );
      s1 += VDot( wvB, trqB );
      s2 += VWLenSq( ssdInertia, wvB );
    } else if ( site[ n ].mechType == SRB ) {
      MVMul( trqB, siteRotationMatrix[ n ].u, siteTorque[ n ] );   /* getting the body torque T^b = R * T^S */
      VSAdd( uB, siteAngularMomentum[ n ], halfDeltaT, trqB ); // half-advance momenta of inertia (body principal frame) from t to t+dt/2
      VSCopy( wvB, 1. / inertia[ site[ n ].type ], uB ); // get angular velocity
      s1 += VDot( wvB, trqB);
      s2 += inertia[ site[ n ].type ] * VLenSq( wvB );
    }
  }
  vFac = - s1 / s2;                                      // eq 6.3.5
  DO_SITE {
    VSAdd( vt, siteVelocity[ n ], halfDeltaT / mass[ site[ n ].type ], siteForce[ n ] ); // half-advance linear velocities
    VVSAdd( siteForce[ n ], mass[ site[ n ].type ] * vFac, vt );                     // eq 6.3.3
    if ( site[ n ].mechType == NSRB || site[ n ].mechType == SRB  ) {
      MVMul( trqB, siteRotationMatrix[ n ].u, siteTorque[ n ] );   /* getting the body torque T^b = R * T^S */
      VSAdd( uB, siteAngularMomentum[ n ], halfDeltaT, trqB );            /* half-advance momenta of inertia (body principal frame) from t to t+dt/2 */
      MVMulT( uS, siteRotationMatrix[ n ].u, uB ); // back to lab-frame
      VVSAdd( siteTorque[ n ], vFac, uS );
    }
  }
}

